#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <yaml-cpp/yaml.h>

// 定义标定参数结构体
struct CalibrationParams
{
  cv::Mat camera_matrix;  // 相机内参矩阵
  cv::Mat dist_coeffs;    // 相机畸变系数
  Eigen::Matrix4d lidar_to_camera_transform;  // 激光雷达到相机坐标系的变换矩阵
};

CalibrationParams readCalibrationParams(const std::string& calib_file)
{
  CalibrationParams params;

  // 从YAML文件中读取标定参数
  YAML::Node calib_data = YAML::LoadFile(calib_file);
  params.camera_matrix = (cv::Mat_<double>(3, 3) <<
                          calib_data["camera_matrix"]["data"][0].as<double>(), calib_data["camera_matrix"]["data"][1].as<double>(), calib_data["camera_matrix"]["data"][2].as<double>(),
                          calib_data["camera_matrix"]["data"][3].as<double>(), calib_data["camera_matrix"]["data"][4].as<double>(), calib_data["camera_matrix"]["data"][5].as<double>(),
                          calib_data["camera_matrix"]["data"][6].as<double>(), calib_data["camera_matrix"]["data"][7].as<double>(), calib_data["camera_matrix"]["data"][8].as<double>());
  params.dist_coeffs = (cv::Mat_<double>(1, 5) <<
                        calib_data["distortion_coefficients"]["data"][0].as<double>(), calib_data["distortion_coefficients"]["data"][1].as<double>(), calib_data["distortion_coefficients"]["data"][2].as<double>(),
                        calib_data["distortion_coefficients"]["data"][3].as<double>(), calib_data["distortion_coefficients"]["data"][4].as<double>());

  Eigen::Matrix<double, 3, 3> rotation_matrix;
  Eigen::Vector3d translation_vector;
  rotation_matrix << calib_data["transform"]["rotation"]["data"][0].as<double>(), calib_data["transform"]["rotation"]["data"][1].as<double>(), calib_data["transform"]["rotation"]["data"][2].as<double>(),
                     calib_data["transform"]["rotation"]["data"][3].as<double>(), calib_data["transform"]["rotation"]["data"][4].as<double>(), calib_data["transform"]["rotation"]["data"][5].as<double>(),
                     calib_data["transform"]["rotation"]["data"][6].as<double>(), calib_data["transform"]["rotation"]["data"][7].as<double>(), calib_data["transform"]["rotation"]["data"][8].as<double>();
  translation_vector << calib_data["transform"]["translation"]["data"][0].as<double>(), calib_data["transform"]["translation"]["data"][1].as<double>(), calib_data["transform"]["translation"]["data"][2].as<double>();

  params.lidar_to_camera_transform.setIdentity();
  params.lidar_to_camera_transform.block<3, 3>(0, 0) = rotation_matrix;
  params.lidar_to_camera_transform.block<3, 1>(0, 3) = translation_vector;

  return params;
}

// 回调函数，接收点云数据
void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg, const CalibrationParams& params)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg(*cloud_msg, *cloud);

  // 将点云转换到相机坐标系中
  pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::transformPointCloud(*cloud, *transformed_cloud, params.lidar_to_camera_transform);

  // 在此处添加代码，将点云投影到相机图像上，并进行标记

  // 将结果发布到新的话题中
  // 在此处添加代码，将投影结果转换为ROS图像消息，并发布到相应的话题上
}

// 回调函数，接收相机图像数据
void imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const CalibrationParams& params)
{
  cv_bridge::CvImagePtr cv_ptr;
  try
  {
    cv_ptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
  }

  // 在此处添加代码，根据相机图像数据进行处理，例如进行标定

  // 在此处添加代码，将标定结果应用于点云
  pointCloudCallback(cloud_msg, params);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "fusion_node");
  ros::NodeHandle nh;

  // 读取标定参数文件
  std::string calib_file = "calibrate.yaml";  // 替换为实际的参数文件路径
  CalibrationParams params = readCalibrationParams(calib_file);

  // 订阅点云数据话题
  ros::Subscriber point_cloud_sub = nh.subscribe<sensor_msgs::PointCloud2>("/point_cloud_topic", 1, boost::bind(pointCloudCallback, _1, params));

  // 订阅相机图像数据话题
  ros::Subscriber image_sub = nh.subscribe<sensor_msgs::Image>("/image_topic", 1, boost::bind(imageCallback, _1, params));

  // 创建新的话题发布器，用于发布投影结果
  ros::Publisher projected_image_pub = nh.advertise<sensor_msgs::Image>("/projected_image_topic", 1);

  ros::spin();

  return 0;
}
